#pragma once
#ifndef _BACKEND_H_
#define _BACKEND_H_

#include "common_include.h"
#include "frame.h"
#include"utility.h"

namespace vld_imu_slam{

class Backend {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Backend> Ptr;
    typedef unordered_map<unsigned long, Frame::Ptr> KeyFramesType;
  
    // Backend();
    Backend(){
        gtsam::ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam_ = new gtsam::ISAM2(parameters);
    }
    void lidarOdomBackendOpt(KeyFramesType& keyframes,vector<LoopFactor>& loopfactor_v);
private:
    gtsam::NonlinearFactorGraph gtSAMgraph_;
    gtsam::Values initialEstimate_;
    gtsam::ISAM2 *isam_;

    void addOdomFactor(KeyFramesType& keyframes);

    bool addLoopFactor(vector<LoopFactor>& loopfactor_v);

};


}
#endif



